#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <time.h>
#include <iostream>
#include <math.h>
#include <stdio.h>
#include <GL/glew.h> 
#include <GL/freeglut.h> 
#include <GL/gl.h> 
#include <GL/glu.h>
#include <fstream>
#include <list>
#include <vector>
#include <math.h>

using namespace std;
using namespace cv;

int start = 0;
VideoCapture cap(0);
Mat frame;
Mat prev;
GLuint program_object_sobel;
GLuint program_object_blur;
GLuint program_object_threshold;
GLuint program_object_temporal;
GLuint vertex_shader;
GLuint fragment_shader_sobel;
GLuint fragment_shader_blur;
GLuint fragment_shader_threshold;
GLuint fragment_shader_temporal;
GLuint tex1, tex2;
GLuint fboTex1, fboTex2, fboTex3, fboTex4, fboTex5, fboTex6;
GLuint fbo_id = 0;
int range = 13000;
GLuint depthbuffer = 0;
int foo = 0;
int mode = 0;

class Cluster
{
    public:
    int x;
    int y;
    int top;
    int bottom;
    int left;
    int right;

    Cluster()
    {
    }
    

    Cluster(int x, int y)
    {
        this->x = x;
        this->y = y;
        this->top = y;
        this->bottom = y;
        this->left = x;
        this-> right = x;
    }    

    Cluster(int x, int y, int top, int bottom, int left, int right)
    {
        this->x = x;
        this->y = y;
        this->top = top;
        this->bottom = bottom;
        this->left = left;
        this-> right = right;
    }
};

string readFile(const string fileName)
{
    ifstream shaderFile(fileName.c_str());
    shaderFile.seekg(0, ios::end);
    streampos length = shaderFile.tellg();
    shaderFile.seekg(0, ios::beg);
    
    vector<char> buffer(length);
    shaderFile.read(&buffer[0], length);
    return string(buffer.begin(),buffer.end());
}

static void printProgramInfoLog(GLuint obj)
{
	GLint infologLength = 0, charsWritten = 0;
	glGetProgramiv(obj, GL_INFO_LOG_LENGTH, &infologLength);
	if (infologLength > 2) {
		GLchar* infoLog = new GLchar [infologLength];
		glGetProgramInfoLog(obj, infologLength, &charsWritten, infoLog);
		std::cerr << infoLog << std::endl;
		delete infoLog;
	}
}


void createTex(int tex, GLuint texId, Mat frame){
    glActiveTexture(tex);
    glBindTexture(GL_TEXTURE_RECTANGLE_ARB, texId);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
    glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
    if(frame.channels() == 3)
        glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGB, frame.size().width, frame.size().height, 0 ,GL_BGR, GL_UNSIGNED_BYTE, frame.data);
    else
        glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA, frame.size().width, frame.size().height, 0 ,GL_BGRA, GL_UNSIGNED_BYTE, frame.data);
}


void setupDraw(void){
    if(!frame.empty())
    {
        prev = frame.clone();
    }
    cap >> frame;
    
    createTex(GL_TEXTURE0, tex1, frame);
    createTex(GL_TEXTURE1, tex2, prev);
}


void draw(void){
    int width = frame.size().width;
    int height = frame.size().height;
    glBegin(GL_POLYGON);
    glTexCoord2i(width, 0);
    glVertex3f(-1,1,0);
    glTexCoord2i(0, 0);
    glVertex3f(1,1,0);
    glTexCoord2i(0, height);
    glVertex3f(1,-1,0);
    glTexCoord2i(width, height);
    glVertex3f(-1,-1,0);
    glEnd();
    glFinish();    
}

void drawInverted(void){
    int width = frame.size().width;
    int height = frame.size().height;
    glBegin(GL_POLYGON);
    glTexCoord2i(0, height);
    glVertex3f(-1,1,0);
    glTexCoord2i(width-1, height);
    glVertex3f(1,1,0);
    glTexCoord2i(width, 0);
    glVertex3f(1,-1,0);
    glTexCoord2i(0, 0);
    glVertex3f(-1,-1,0);
    glEnd();
    glFinish();    
}

void createTexFB(GLuint &texId, int attachId){
    glGenTextures(1, &texId);
    glBindTexture(GL_TEXTURE_RECTANGLE_ARB, texId);
    glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA, frame.size().width, frame.size().height, 0, GL_RGBA, GL_UNSIGNED_BYTE, 0);

    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

    glFramebufferTexture2D(GL_FRAMEBUFFER, attachId, GL_TEXTURE_RECTANGLE_ARB, texId , 0);
}

void createFB(void){
    glGenFramebuffers(1, &fbo_id);
    glBindFramebuffer(GL_FRAMEBUFFER, fbo_id);
  
    createTexFB(fboTex1, GL_COLOR_ATTACHMENT0);
    createTexFB(fboTex2, GL_COLOR_ATTACHMENT1);
    createTexFB(fboTex3, GL_COLOR_ATTACHMENT2);
    createTexFB(fboTex4, GL_COLOR_ATTACHMENT3);
    createTexFB(fboTex5, GL_COLOR_ATTACHMENT4);
    createTexFB(fboTex6, GL_COLOR_ATTACHMENT5);
                
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    glBindRenderbuffer(GL_RENDERBUFFER, 0);
    glBindTexture(GL_TEXTURE_RECTANGLE_ARB, 0);
}

void bindFB(void){
    glBindFramebuffer(GL_FRAMEBUFFER, fbo_id);
}

void unbindFB(void){
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
}

void bindAttachment (int tex, GLuint fboTex) {
    glActiveTexture(tex);
    glBindTexture(GL_TEXTURE_RECTANGLE_ARB, fboTex);
}

void unbindAttachment (int tex) {
    glActiveTexture(tex);
    glBindTexture(GL_TEXTURE_RECTANGLE_ARB, 0);
}

void drawEmptyRect(float top, float bottom, float left, float right)
{
    glBegin(GL_LINE_LOOP);
    glVertex3f(left,top,0);
    glVertex3f(right,top,0);
    glVertex3f(right,bottom,0);
    glVertex3f(left,bottom,0);
    glEnd();    
}

void renderScene(void){	
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();

    GLuint texLoc;
        
    setupDraw();

  	glUseProgram(program_object_sobel);

    bindFB();

    GLenum buffers1[] = { GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT2 };
    GLenum buffers2[] = { GL_COLOR_ATTACHMENT3, GL_COLOR_ATTACHMENT1 };
        
    if(foo == 0)
    {
        glDrawBuffers(2, buffers1);
        bindAttachment(GL_TEXTURE1, fboTex2);
        foo = 1;
    }
    else
    {
        glDrawBuffers(2, buffers2);
        bindAttachment(GL_TEXTURE1, fboTex3);
        foo = 0;    
    }

    drawInverted();

    if(foo == 1)
    {
        bindAttachment(GL_TEXTURE0, fboTex1);
        bindAttachment(GL_TEXTURE1, fboTex4);    
    }
    
    else
    {
        bindAttachment(GL_TEXTURE0, fboTex4);
        bindAttachment(GL_TEXTURE1, fboTex1);        
    }    
    
  	glUseProgram(program_object_temporal);
    
	texLoc = glGetUniformLocation(program_object_sobel, "tex");
    glUniform1i(texLoc, 0);
	texLoc = glGetUniformLocation(program_object_sobel, "texOld");
    glUniform1i(texLoc, 1);
    
    glDrawBuffer(GL_COLOR_ATTACHMENT4);

    drawInverted();




  	glUseProgram(program_object_blur);

    bindAttachment(GL_TEXTURE0, fboTex5);
    
    glDrawBuffer(GL_COLOR_ATTACHMENT5);

    drawInverted();


    glReadBuffer(GL_COLOR_ATTACHMENT5);
    
    float pixels[480][640][4];
    
    glReadPixels(0, 0, 640, 480, GL_RGBA, GL_FLOAT, &pixels);
    
    
    vector<Cluster> centroids;
    
    for(int i = 0; i < 640; i++)
    {
        for(int j = 0; j < 480; j++)
        {
            if(pixels[j][i][0] > 0.75)
            {
                centroids.push_back(Cluster(i,j));
            }
        }
    }
    
    vector<Cluster> centroids2;    
    
    int x = 0;
    int y = 0;
    int count = 0;
    
    if(centroids.size() < 1000)
    for(int n = 0; n < 3; n++)
    {
        for(int i = 0; i < centroids.size(); i++)
        {
            x = 0;
            y = 0;
            count = 0;
            for(int j = 0; j < centroids.size(); j++)
            {
                if (pow(centroids[i].x - centroids[j].x,2) + pow(centroids[i].y - centroids[j].y,2) < range)
                {
                    if(centroids[j].right > centroids[i].right)
                        centroids[i].right = centroids[j].right;
                    if(centroids[j].left < centroids[i].left)
                        centroids[i].left = centroids[j].left;
                    if(centroids[j].top < centroids[i].top)
                        centroids[i].top = centroids[j].top;
                    if(centroids[j].bottom > centroids[i].bottom)
                        centroids[i].bottom = centroids[j].bottom;

                    x += centroids[j].x;
                    y += centroids[j].y;
                    count++;
                }    
            }
            if(count > 0)
            {
                centroids2.push_back(Cluster(x/count, y/count, centroids[i].top, centroids[i].bottom, centroids[i].left, centroids[i].right));
            }
        }
        
        centroids.clear();
        //centroids2.erase(unique(centroids2.begin(), centroids2.end()),centroids2.end());
        
        for(int i = 0; i < centroids2.size(); i+=2)
        {
            x = 0;
            y = 0;
            count = 0;
            for(int j = 0; j < centroids2.size(); j+=2)
            {
                if (pow(centroids2[i].x - centroids2[j].x,2) + pow(centroids2[i].y - centroids2[j].y,2) < range)
                {
                    if(centroids2[j].right > centroids2[i].right)
                        centroids2[i].right = centroids2[j].right;
                    if(centroids2[j].left < centroids2[i].left)
                        centroids2[i].left = centroids2[j].left;
                    if(centroids2[j].top < centroids2[i].top)
                        centroids2[i].top = centroids2[j].top;
                    if(centroids2[j].bottom > centroids2[i].bottom)
                        centroids2[i].bottom = centroids2[j].bottom;                
                
                    x += centroids2[j].x;
                    y += centroids2[j].y;
                    count++;
                }    
            }
            if(count > 0)
            {
                centroids.push_back(Cluster(x/count, y/count, centroids2[i].top, centroids2[i].bottom, centroids2[i].left, centroids2[i].right));
            }
        }        
        
        centroids2.clear();
        //centroids.erase(unique(centroids.begin(), centroids.end()),centroids.end());        
        //cout << centroids.size() << " ";
    }
    
    unbindFB();
    
	glUseProgram(0);
	
	switch(mode)
	{
        case(1):  
            glActiveTexture(GL_TEXTURE0);
            glBindTexture(GL_TEXTURE_RECTANGLE_ARB, tex1);
	        break;	
        case(2):  
	        if(foo == 1) bindAttachment(GL_TEXTURE0,fboTex1); 
            else bindAttachment(GL_TEXTURE0,fboTex4);
	        break;	
	    case(3):  
            bindAttachment(GL_TEXTURE0,fboTex5);
	        break;
    	case(4): 
    	    bindAttachment(GL_TEXTURE0,fboTex6);
    	    break;
	}
	
	draw();
	
	float fx, fy, top, left, right, bottom;
	if(centroids.size() < 1000)
	for(int i = 0; i < centroids.size(); i++)
    {
        right = (1.0 - ((centroids[i].right)/640.0) - 0.5)*2;
        left = (1.0 - ((centroids[i].left)/640.0) - 0.5)*2;
        top = (1.0 - ((centroids[i].top)/480.0) - 0.5)*2;
        bottom = (1.0 - ((centroids[i].bottom)/480.0) - 0.5)*2;
	    glColor3f(1.0,0.0,0.0);
        drawEmptyRect(top, bottom, left, right);
    }
	
    glutSwapBuffers();
}

void init(){
    cap >> frame;
    
    createFB();

    program_object_sobel = glCreateProgram();
    program_object_threshold = glCreateProgram();
    program_object_blur = glCreateProgram();
    program_object_temporal = glCreateProgram();
    vertex_shader  = glCreateShader(GL_VERTEX_SHADER);
    fragment_shader_sobel = glCreateShader(GL_FRAGMENT_SHADER);
    fragment_shader_threshold = glCreateShader(GL_FRAGMENT_SHADER);
    fragment_shader_blur = glCreateShader(GL_FRAGMENT_SHADER);
    fragment_shader_temporal = glCreateShader(GL_FRAGMENT_SHADER);
            
    const string vs = readFile("regular.vert");
    const GLchar *vertexShader = vs.c_str();
    
    const string fs1 = readFile("thresh.frag");
    const GLchar *fragmentShader1 = fs1.c_str();
    
    const string fs2 = readFile("sobel.frag");
    const GLchar *fragmentShader2 = fs2.c_str();
    
    const string fs3 = readFile("blur.frag");
    const GLchar *fragmentShader3 = fs3.c_str();
    
    const string fs4 = readFile("temporal.frag");
    const GLchar *fragmentShader4 = fs4.c_str();        
    
    glShaderSource(vertex_shader, 1, &vertexShader, NULL);
    glShaderSource(fragment_shader_threshold, 1, &fragmentShader1, NULL);
    glShaderSource(fragment_shader_sobel, 1, &fragmentShader2, NULL);   
    glShaderSource(fragment_shader_blur, 1, &fragmentShader3, NULL);   
    glShaderSource(fragment_shader_temporal, 1, &fragmentShader4, NULL);   
           
    glCompileShader(vertex_shader);
    glAttachShader(program_object_threshold, vertex_shader); 
    glAttachShader(program_object_sobel, vertex_shader); 
    glAttachShader(program_object_blur, vertex_shader); 
    glAttachShader(program_object_temporal, vertex_shader);        
   
    glCompileShader(fragment_shader_threshold);
    glAttachShader(program_object_threshold, fragment_shader_threshold);     

    glCompileShader(fragment_shader_sobel);
    glAttachShader(program_object_sobel, fragment_shader_sobel); 
    
    glCompileShader(fragment_shader_blur);
    glAttachShader(program_object_blur, fragment_shader_blur); 
    
    glCompileShader(fragment_shader_temporal);
    glAttachShader(program_object_temporal, fragment_shader_temporal); 
        
    
    glLinkProgram(program_object_threshold);
    glLinkProgram(program_object_sobel);
    glLinkProgram(program_object_blur);
    glLinkProgram(program_object_temporal);        

  	glUseProgram(program_object_threshold);	        
	GLint texLoc = glGetUniformLocation(program_object_threshold, "tex");
    glUniform1i(texLoc, 0);
    
    glUseProgram(program_object_sobel);	        
	texLoc = glGetUniformLocation(program_object_sobel, "inputTexture");
    glUniform1i(texLoc, 0);
	texLoc = glGetUniformLocation(program_object_sobel, "potentialTexture");
    glUniform1i(texLoc, 1);    

  	glUseProgram(program_object_blur);    
	texLoc = glGetUniformLocation(program_object_blur, "tex");
    glUniform1i(texLoc, 0);

  	glUseProgram(program_object_temporal);
	texLoc = glGetUniformLocation(program_object_temporal, "tex");
    glUniform1i(texLoc, 0);        
 	GLint texOldLoc = glGetUniformLocation(program_object_temporal, "texOld");
	glUniform1i(texOldLoc, 1);
    
    glGenTextures(1, &tex1);
    glGenTextures(1, &tex2);    
}

void keyPressed(unsigned char key, int x, int y)
{
    if(key == '+' && range < 16000)
    {
        range += 500;
        cout << "Range: " << range << endl;
    }

    if(key == '-' && range > 600)
    {
        range -= 500;
        cout << "Range: " << range << endl;
    }

    if(key == '1')
    {
        mode = 1;
        cout << "Regular mode" << endl;
    }
    
    if(key == '2')
    {
        mode = 2;
        cout << "Parvocelullar mode" << endl;
    }
    
    if(key == '3')
    {
        mode = 3;
        cout << "Magnocelullar mode" << endl;
    }
    
    if(key == '4')
    {
        mode = 4;
        cout << "Magno post-erode mode" << endl;
    }        
}

int main(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    glutInitWindowPosition(100,100);
    glutInitWindowSize(640,480);
    glutCreateWindow("GLSL");
    glutDisplayFunc(renderScene);
    glutIdleFunc(renderScene);
    glutKeyboardFunc(keyPressed);
    glewInit();
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_RECTANGLE_ARB);
    glClearColor(0.0,0.0,0.0,1.0);
    init();
    glutMainLoop();
    
    return 0;
}







